#include <stdio.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "esp_log.h"
#include "driver/gpio.h"

#include "tmc2209.h"
#include "led.h"

#define KEY_UP_PIN 9
#define KEY_DOWN_PIN 2

static QueueHandle_t gpio_evt_queue = NULL;

static void IRAM_ATTR gpio_isr_handler(void* arg)
{
    uint32_t gpio_num = (uint32_t) arg;
    xQueueSendFromISR(gpio_evt_queue, &gpio_num, NULL);
}

static void gpio_task_example(void* arg)
{
    uint32_t io_num;
    tmc_queue_t tmcq;

    for (;;) {
        if (xQueueReceive(gpio_evt_queue, &io_num, portMAX_DELAY)) {
            gpio_intr_disable(io_num);
            ESP_LOGI("key", "GPIO[%"PRIu32"] intr, val: %d\n", io_num, gpio_get_level(io_num));
            vTaskDelay(50 / portTICK_PERIOD_MS);

            while (gpio_get_level(io_num) == 0) {
                vTaskDelay(20 / portTICK_PERIOD_MS);
            }

            if (get_tmc_status()) {
                memset(&tmcq, 0, sizeof(tmc_queue_t));

                tmcq.type = TMC_STOP;
                xQueueSend(tmc_queue, &tmcq, pdMS_TO_TICKS(200));
                led_queuesend_state(LED_FLASH);
            } else if (io_num == KEY_UP_PIN) {
                memset(&tmcq, 0, sizeof(tmc_queue_t));

                tmcq.type = TMC_TARGET;
                tmcq.data.val = 0;
                xQueueSend(tmc_queue, &tmcq, pdMS_TO_TICKS(200));
                led_queuesend_state(LED_FLASH);
            } else if (io_num == KEY_DOWN_PIN) {
                memset(&tmcq, 0, sizeof(tmc_queue_t));

                tmcq.type = TMC_TARGET;
                tmcq.data.val = 100;
                xQueueSend(tmc_queue, &tmcq, pdMS_TO_TICKS(200));
                led_queuesend_state(LED_FLASH);
            }

            gpio_intr_enable(io_num);
        }
    }
}

void key_init(void)
{
    gpio_config_t io_conf = {};

    ESP_LOGI("key_init", "key init");

    //interrupt of rising edge
    io_conf.intr_type = GPIO_INTR_NEGEDGE;
    //bit mask of the pins, use GPIO4/5 here
    io_conf.pin_bit_mask = ((1ULL << KEY_UP_PIN) | (1ULL << KEY_DOWN_PIN));
    //set as input mode
    io_conf.mode = GPIO_MODE_INPUT;
    //enable pull-up mode
    io_conf.pull_up_en = 1;
    gpio_config(&io_conf);

    gpio_install_isr_service(0);

    gpio_isr_handler_add(KEY_UP_PIN, gpio_isr_handler, (void*) KEY_UP_PIN);
    //hook isr handler for specific gpio pin
    gpio_isr_handler_add(KEY_DOWN_PIN, gpio_isr_handler, (void*) KEY_DOWN_PIN);

    gpio_evt_queue = xQueueCreate(10, sizeof(uint32_t));
    xTaskCreate(gpio_task_example, "gpio_task_example", 4096, NULL, 10, NULL);
}